#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include <iostream>

using NavigateToPose = nav2_msgs::action::NavigateToPose;
using GoalHandle = rclcpp_action::ClientGoalHandle<NavigateToPose>;

class NavClient : public rclcpp::Node
{
public:
    NavClient() : Node("nav2_cpp_client")
    {
        // 创建导航动作客户端
        client_ = rclcpp_action::create_client<NavigateToPose>(this, "navigate_to_pose");
        
        // 创建初始位姿发布器
        initial_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "initialpose", 10);
        
        // 等待Nav2动作服务器启动
        RCLCPP_INFO(this->get_logger(), "等待Nav2服务器启动...");
        while (!client_->wait_for_action_server(std::chrono::seconds(1))) {
            if (!rclcpp::ok()) {
                RCLCPP_ERROR(this->get_logger(), "程序被用户中断");
                exit(0);
            }
            RCLCPP_INFO(this->get_logger(), "仍在等待Nav2服务器...");
        }
        RCLCPP_INFO(this->get_logger(), "Nav2服务器已连接");
        
        // 初始化位姿为原点 (0,0,0)
        initialize_pose_at_origin();
    }

    // 初始化机器人位姿为原点
    void initialize_pose_at_origin()
    {
        auto init_pose = geometry_msgs::msg::PoseWithCovarianceStamped();
        init_pose.header.frame_id = "map";
        init_pose.header.stamp = this->now();
        
        // 设置位姿为原点 (x=0, y=0, z=0, 无旋转)
        init_pose.pose.pose.position.x = 0.0;
        init_pose.pose.pose.position.y = 0.0;
        init_pose.pose.pose.position.z = 0.0;
        init_pose.pose.pose.orientation.x = 0.0;
        init_pose.pose.pose.orientation.y = 0.0;
        init_pose.pose.pose.orientation.z = 0.0;
        init_pose.pose.pose.orientation.w = 1.0;
        
        // 设置协方差矩阵（对角元素表示不确定性）
        for (size_t i = 0; i < 36; ++i) init_pose.pose.covariance[i] = 0.0;
        init_pose.pose.covariance[0] = 0.25;  // x方向不确定性
        init_pose.pose.covariance[7] = 0.25;  // y方向不确定性
        init_pose.pose.covariance[35] = 0.06853891945200942;  // 朝向不确定性
        
        // 发布初始位姿（需多次发布确保被接收）
        RCLCPP_INFO(this->get_logger(), "初始化机器人位姿为原点 (0,0,0)");
        for (int i = 0; i < 5; ++i) {
            initial_pose_pub_->publish(init_pose);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
        
        // 等待一小段时间让系统处理初始位姿
        RCLCPP_INFO(this->get_logger(), "位姿初始化完成，等待系统处理...");
        rclcpp::sleep_for(std::chrono::seconds(2));
    }
    
    // 发送导航目标点
    void send_goal(double x, double y)
    {
        // 构造目标点消息
        auto goal_msg = NavigateToPose::Goal();
        goal_msg.pose.header.frame_id = "map";
        goal_msg.pose.header.stamp = this->now();
        goal_msg.pose.pose.position.x = x;
        goal_msg.pose.pose.position.y = y;
        goal_msg.pose.pose.position.z = 0.0;
        goal_msg.pose.pose.orientation.x = 0.0;
        goal_msg.pose.pose.orientation.y = 0.0;
        goal_msg.pose.pose.orientation.z = 0.0;
        goal_msg.pose.pose.orientation.w = 1.0;

        // 设置发送目标的选项
        auto send_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
        send_options.feedback_callback = 
            [this](GoalHandle::SharedPtr, const std::shared_ptr<const NavigateToPose::Feedback> feedback) {
                RCLCPP_INFO(this->get_logger(), "剩余距离: %.4f米", feedback->distance_remaining);
            };
        send_options.result_callback = 
            [this](const GoalHandle::WrappedResult & result) {
                switch (result.code) {
                    case rclcpp_action::ResultCode::SUCCEEDED:
                        RCLCPP_INFO(this->get_logger(), "导航成功！");
                        break;
                    case rclcpp_action::ResultCode::ABORTED:
                        RCLCPP_ERROR(this->get_logger(), "导航失败（被中止）");
                        break;
                    case rclcpp_action::ResultCode::CANCELED:
                        RCLCPP_WARN(this->get_logger(), "导航任务已取消");
                        break;
                    default:
                        RCLCPP_ERROR(this->get_logger(), "导航结果未知");
                        break;
                }
                task_complete_ = true;
            };

        // 发送目标并重置任务状态
        task_complete_ = false;
        auto goal_handle_future = client_->async_send_goal(goal_msg, send_options);
        
        // 等待目标被接收
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), goal_handle_future) !=
            rclcpp::FutureReturnCode::SUCCESS) {
            RCLCPP_ERROR(this->get_logger(), "发送目标失败");
            task_complete_ = true;
        }
    }

    // 检查任务是否完成
    bool is_task_complete() const { return task_complete_; }

private:
    rclcpp_action::Client<NavigateToPose>::SharedPtr client_;
    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
    bool task_complete_ = false;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<NavClient>();

    // 循环接收用户输入并导航
    while (rclcpp::ok()) {
        double x, y;
        std::cout << "\n请输入目标点坐标：" << std::endl;
        std::cout << "x: ";
        std::cin >> x;
        std::cout << "y: ";
        std::cin >> y;

        // 发送导航目标
        node->send_goal(x, y);

        // 等待导航完成
        while (!node->is_task_complete() && rclcpp::ok()) {
            rclcpp::spin_some(node);
            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }
    }

    rclcpp::shutdown();
    return 0;
}
